#ifndef __DRV_ALGO_H
#define __DRV_ALGO_H
#include "main.h"

// drv_algo.h 中的结构体定义
typedef struct {
    // 基础PID参数
    float Kp;
    float Ki;
    float Kd;
    float error_int;        // 积分项
    float last_error;       // 上次误差
    
    // 双向限幅参数
    float max_iout;         // 积分输出限幅（双向，绝对值）
    float max_out;          // 总输出限幅（双向，绝对值）
    
} algo_pid_t;


void Algo_PID_Init(algo_pid_t* pPID, float Kp, float Ki, float Kd, float max_iout, float max_out);
float Algo_PID_Calc(algo_pid_t* pPID, float actual, float target);












typedef struct _pid_struct_t
{
  float kp;//比例
  float ki;//积分
  float kd;//微分
  float i_max;//积分限幅
  float out_max;//输出限幅
  
  float ref;      // target value目标角度
  float fdb;      // feedback value设定角度
  float err[2];   // error and last error差值
 
  float p_out;//比例输出
  float i_out;//积分输出
  float d_out;//微分输出
  float output;//pid总输出
}pid_struct_t;
 
void pid_init(pid_struct_t *pid,
              float kp,
              float ki,
              float kd,
              float i_max,
              float out_max);
 
void gimbal_PID_init(void);
float pid_calc(pid_struct_t *pid, float ref, float fdb);

#endif
